#ifndef STANLEY_CONTROLLER_H_
#define  STANLEY_CONTROLLER_H_

#include <sstream>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <std_srvs/Empty.h>
#include <vector>
#include <random>
#include <iostream>


using path_type = std::pair<std::pair<double, double>, double>;

namespace stanley_control
{
class StanleyController
{
    public:
        StanleyController(ros::NodeHandle& n): n_(n){};
        void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose);
        void robotPathCallBack(const nav_msgs::PathConstPtr& path_msg);
        void run();
        
        auto GetMinDisIndex(path_type current_pose, std::vector<path_type> path) -> int;
        auto distance(path_type pos1, path_type pos2) -> double ;

        void OdomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg);


        ~StanleyController(){};
    public:

    private:
        ros::NodeHandle& n_;
        double factor = 0.8;
        double speed = 0.3;
        bool recv_path_flag;

        tf::Point current_pose;
        double yaw;
        double vel_x,vel_z;
        ros::Publisher cmd_vel_pub, real_path_pub;
        ros::Subscriber pose_sub, odom_pose_sub;
        ros::Subscriber path_sub;
        nav_msgs::Path nav_path_;
        geometry_msgs::Twist vel;
        geometry_msgs::Pose start_pose_;
        nav_msgs::Path path;
        std::vector<path_type> path_vector;        

};
}

#endif   // STANLEY_CONTROLLER_H_
